#include "inc/hw_types.h"		// tBoolean
#include "inc/hw_memmap.h"
#include "RASLib/init.h"
#include "driverlib/gpio.h"
#include "driverlib/sysctl.h"

#include "RASLib/motor.h"
#include "RASLib/encoder.h"

void testTime(int duration);
void testEncoders(int endValue);
void Wait(int);
void initMotors(void);
void initEncoders(void);


void test(){
	int motorDuration=1000; 
	LockoutProtection();
	InitializeMCU();  
	initMotors();	   
	initEncoders();
	

	testTime(motorDuration);
	//testEncoders(encoderDuration);
	SetMotorPowers(0,0);
	
}	

/**
 *This method tests to see how far the robot goes per unit of time
 *Result: 
 */
void testTime(int duration){  
	int maxSpeed=127;	   

	SetMotorPowers(maxSpeed, maxSpeed);
	Wait(duration);

	SetMotorPowers(0,0);
} 

/**
 *This method tests to see how far the robot goes per encoder value
 *Result: 
 */
void testEncoders(int endValue){
	int maxSpeed=127;
	signed long encoder0, encoder1;
	InitializeEncoders(true, false);
	PresetEncoderCounts(0, 0);

	SetMotorPowers(maxSpeed, maxSpeed);
	while(1){
		encoder0 = GetEncoderCount(ENCODER_0);	 
		encoder1 = GetEncoderCount(ENCODER_1);
		GetEncoderCounts(&encoder0, &encoder1);
		if(encoder0>=endValue||encoder0<=-endValue)break;
	}

	SetMotorPowers(0,0);
}
